import time
from apm_quad_x import apm_quad_x
import socket
import threading
import atexit

def out():
    print('结束')
atexit.register(out)
order_dict = {
    -1:"land",
    0:"NULL",
    1:"takeoff",
    2:"hover",
    3:"forward",
    4:"backward",
    5:"rot_left",
    6:"rot_right"
    }



            
class udp_bridge():
    def __init__(self):
        self.flag_land_takeoff = 0
        self.udpSocket2 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.command = "NULL"
        self.last_command_recv_time = 0
        addr = ('10.42.0.38', 6666)
        # 本机地址
        self.udpSocket2.bind(addr)
        # self.udpSocket2.settimeout(10)

        t1 = threading.Thread(target=self.udp_rcv)
        t1.start()
    def udp_rcv(self):
        while True:
            try:
                buffer = self.udpSocket2.recv(64)
                ## 判断帧头和数据长度
                if buffer[0] == 0x55 and buffer[1] == 0xEE and buffer[2] == 0x05 and len(buffer) >= 9:

                    checkSum = 0
                    ## 计算校验和
                    for iNum in range(8):
                        checkSum += int(buffer[iNum])

                    ## 检查校验和
                    if checkSum & 0xff == buffer[8]:
                        ##获取并判断指令
                        data = buffer[5:8]

                        if data[2] == 0x01:
                            if(self.flag_land_takeoff == 0):
                                order = 1
                                self.flag_land_takeoff=1
                            elif(self.flag_land_takeoff == 1):
                                order = -1
                                self.flag_land_takeoff=0 
                        elif data[2] == 0x02:
                            order = 2
                        elif data[2] == 0x04:
                            order = 3
                        elif data[2] == 0x08:
                            order = 4
                        elif data[2] == 0x10:
                            order = 5
                        elif data[2] == 0x20:
                            order = 6
                        else:
                            order = 0
                        ## 处理指令 @TODO
                        ## only for test
                        print("[UDP RECV] %s" % order_dict[order])
                        self.command=order_dict[order]
                        self.last_command_recv_time = time.time()
            except BaseException as Argument:
                print("[UDP RECV]" + str(Argument))
                print("[UDP RECV]" + "set command NULL")
                self.command="NULL"
            

if __name__=="__main__":
    quad = apm_quad_x('/dev/ttyAMA0')
    udp_recv = udp_bridge()
   
    # while True:
    while quad.link_alive:
        quad.command_run(udp_recv.command)
        quad.task_manager()
       
        time.sleep(0.05)
    # quad.close_mission()
